#include "stdafx.h"
#include "Interpolator.h"

CKrigingInterpolator::CKrigingInterpolator()
	: pProvider(NULL)
	, c0(0)
	, c1(0)
	, a(0)
	, pv(NULL)
	, pd(NULL)
	, pw(NULL)
{
}

CKrigingInterpolator::~CKrigingInterpolator()
{
	if(pv)delete pv;
	if(pd)delete pd;
	if(pw)delete pw;
}

void CKrigingInterpolator::Initialize(IDataProvider* _pProvider)
{
	pProvider = _pProvider;

	CalculateCoeffs();

	//build V matrix
	UINT uiCount = pProvider->GetCount();
	pv = new CArray2D<float>(uiCount + 1, uiCount + 1);
	CArray2D<float>& v = *pv;

	for(UINT i = 0; i < uiCount; ++i)
	{
		for(UINT j = i; j < uiCount; ++j)
		{
			float h = pProvider->GetDistance(i, j);
			v[i][j] = v[j][i] = CalculateVariogram(EVariogramModeSphere, h);
		}
	}

	for(UINT i = 0; i < uiCount; ++i)
	{
		v[uiCount][i] = 1;  //The components of last row are 1
		v[i][uiCount] = 1;  //The components of last column are 1
	}
	v[uiCount][uiCount] = 0; //the last component of the matrix is 0

	pd = new float[uiCount + 1];
	pw = new float[uiCount + 1];
}

float CKrigingInterpolator::Interpolate(void* pos)
{
	//build D vector
	UINT uiCount = pProvider->GetCount();
	for(UINT k = 0; k < uiCount; ++k)
	{
		float h = pProvider->GetDistance(k, pos);
		pd[k] = CalculateVariogram(EVariogramModeSphere, h);
	}
	pd[uiCount] = 1; //The last component of the vector is 1.

	//solve matrix equation to get W
	SolveMatrixEquation(pd, pw);

	//get interpolated result
	float result = 0;
	for(UINT k = 0; k < uiCount; ++k)
	{
		result += pw[k] * pProvider->GetData(k);
	}

	return result;
}

/*
input member variables: pProvider
output member variables: c0, c1, a
*/
void CKrigingInterpolator::CalculateCoeffs() //calcualting c0, c1, a
{
	UINT uiCount = pProvider->GetCount();

	float halfMaxDist = 0;
	int nt = 25;
	float ht = 0;
	for(UINT i = 0; i < uiCount - 1; ++i)
	{
		for(UINT j = i + 1; j < uiCount; ++j)
		{
			float d = pProvider->GetDistance(i, j);
			halfMaxDist = glm::max(halfMaxDist, d / 2.0f);
		}
	}
	ht = halfMaxDist / nt;

	//calculate known variogram result
	//????why using this method????
	SVarigramData* pVarigramData = new SVarigramData[nt];
	::ZeroMemory(pVarigramData, sizeof(SVarigramData) * nt);
	for(UINT i = 0; i < uiCount - 1; ++i)
	{
		for(UINT j = i + 1; j < uiCount; ++j)
		{
			float d = pProvider->GetDistance(i, j);
			if(d > __EPSILON__ && d <= halfMaxDist)
			{
				int hp = (int)round(d / ht);
				float fWeight = 1 - (d - hp * ht) * (d - hp * ht) / ht / ht / 4;
				if(hp > 0)
				{
					pVarigramData[hp - 1].fVarigram += 
						(pProvider->GetData(i) - pProvider->GetData(j)) * 
						(pProvider->GetData(i) - pProvider->GetData(j)) * fWeight;
					pVarigramData[hp - 1].fSumOfWeight += fWeight; 
					++pVarigramData[hp - 1].nCount;
				}
			}
		}
	}
	for(int i = 0; i < nt; ++i)
	{
		if(pVarigramData[i].fSumOfWeight != 0)
		{
			pVarigramData[i].fVarigram /= pVarigramData[i].fSumOfWeight * 2;
		}
	}

	//fitting variogram equation(sphere mode)
	//r = c0 + c1 * (1.5f * h / a + - 0.5f * (h / a) ^ 3)
	//r = b0 + b1 * h + b2 * h^3
	//c0 = b0
	//c1 = b1 / 3.0f * sqrt(b1 / b2 / - 3.0f)
	//a = sqrt(b1 / b2 / - 3.0f)
	//according to following equations, to solve b1 and b2
	//C(h, h) * b1 + C(h, h^3) * b2 = C(h, r)
	//C(h^3, h) * b1 + C(h^3, h^3) * b2 = C(h^3, r)
	//according to following equation, to solve b0
	//b0 = E(r) - b1 * E(h) - b2 * E(h^3)
	int nTotalCount = 0;
	float E_r = 0;
	float E_h = 0;
	float E_h3 = 0;
	float E_h_h = 0;
	float E_h_h3 = 0;
	float E_h3_h3 = 0;
	float E_h_r = 0;
	float E_h3_r = 0;

	for(int i = 0; i < nt; ++i)
	{
		float h = (i + 1) * ht;
		float h3 = h * h * h; 

		nTotalCount += pVarigramData[i].nCount;
		E_r += pVarigramData[i].nCount * pVarigramData[i].fVarigram;
		E_h += pVarigramData[i].nCount * h;
		E_h3 += pVarigramData[i].nCount * h3;
		E_h_h += pVarigramData[i].nCount * h * h;
		E_h_h3 += pVarigramData[i].nCount * h * h3;
		E_h3_h3 += pVarigramData[i].nCount * h3 * h3;
		E_h_r += pVarigramData[i].nCount * h * pVarigramData[i].fVarigram;
		E_h3_r += pVarigramData[i].nCount * h3 * pVarigramData[i].fVarigram;
	}
	E_r /= nTotalCount;
	E_h /= nTotalCount;
	E_h3 /= nTotalCount;

	float C_h_h = E_h_h - E_h * E_h / nTotalCount;
	float C_h_h3 = E_h_h3 - E_h * E_h3 / nTotalCount;
	float C_h3_h3 = E_h3_h3 - E_h3 * E_h3 / nTotalCount;
	float C_h_r = E_h_r - E_h * E_r / nTotalCount;
	float C_h3_r = E_h3_r - E_h3 * E_r / nTotalCount;
	float b0 = 0, b1 = 0, b2 = 0;

	b1 = (C_h_r * C_h3_h3 - C_h3_r * C_h_h3) / (C_h_h * C_h3_h3 - C_h_h3 * C_h_h3);
	b2 = (C_h3_r * C_h_h - C_h_r * C_h_h3) / (C_h_h * C_h3_h3 - C_h_h3 * C_h_h3);
	b0 = E_r - b1 * E_h - b2 * E_h3;
	if(b0 < 0)
	{
		C_h_h = E_h_h;
		C_h_h3 = E_h_h3;
		C_h3_h3 = E_h3_h3;
		C_h_r = E_h_r;
		C_h3_r = E_h3_r;
		b1 = (C_h_r * C_h3_h3 - C_h3_r * C_h_h3) / (C_h_h * C_h3_h3 - C_h_h3 * C_h_h3);
		b2 = (C_h3_r * C_h_h - C_h_r * C_h_h3) / (C_h_h * C_h3_h3 - C_h_h3 * C_h_h3);
		b0 = 0;
	}

	//output parameters
	c0 = b0;
	c1 = b1 * sqrt(-b1 / b2 / 3.0f) / 3.0f;
	a = sqrt(-b1 / b2 / 3.0f); //here comparing with origin impelementation, dividing by 2 is removed.

	delete[] pVarigramData;
}

/*
input member variables: c0, c1, a
output member variables: 
*/
float CKrigingInterpolator::CalculateVariogram(EVariogramMode eMode, float h)
{
	float r = 0;

	switch(eMode)
	{
	case EVariogramModeSphere:
		/*
			r = c0 + c1 * (1.5f * h / a + - 0.5f * (h / a) ^ 3)
			calcuating method is weired???
		*/
		if(h < __EPSILON__)
		{
			r = c0 + c1;
		}
		else if(h <= a)
		{
			r = c1 - c1 * (1.5f * h / a - 0.5f * h * h * h / a / a / a);
		}
		else
		{
			r = 0;
		}
		break;
	case EVariogramModeExponent:
		/*
			r = c0 + c1 * (1 - exp(-3 * |h| / a)
		*/
		r = c0 + c1 * (1 - exp(-3 * abs(h) / a));
		break;
	default:
		assert(FALSE);
	}

	return r;
}

/*
input member variables: pv
output member variables: 
*/
void CKrigingInterpolator::SolveMatrixEquation(const float* d, float* w)
{
	CArray2D<float>& v = *pv;
	int n = (int)v.GetWidth();
	float* y = new float[n];
	CArray2D<float> l(n, n);
	CArray2D<float> u(n, n);

	for(int i = 0; i < n; ++i)
	{
		u[i][i] = 1;
	}

	for(int i = 0; i < n; ++i)
	{
		for(int j = i; j < n; ++j)
		{
			l[j][i] = v[j][i];
			for(int k = 0; k < i; ++k)
			{
				l[j][i] -= l[j][k] * u[k][i];
			}
		}
		for(int j = i + 1; j < n; ++j)
		{
			u[i][j] = v[i][j];
			for(int k = 0; k < i; ++k)
			{
				u[i][j] -= l[i][k] * u[k][j];
			}
			u[i][j] /= l[i][i];
		}
	}

	for(int i = 0; i < n; ++i)
	{
		y[i] = d[i];
		for(int j = 0; j < i; ++j)
		{
			y[i] -= l[i][j] * y[j];
		}
		y[i] /= l[i][i];
	}

	for(int i = n - 1; i >= 0; --i)
	{
		w[i] = y[i];
		for(int j = i + 1; j < n; ++j)
		{
			w[i] -= u[i][j] * w[j];
		}
	}

	delete[] y;
}